#include "particle.h"
#include "main.h"
#define FOR_EACH_ATOM_i    for(int i=0; i< num; i++)
#define Dt 0.005


particle::particle(int t0,  int l0, int u0, real m0): type(t0), ll(l0), lu(u0), mass(m0)
{
    xixx = real(0.);
    xiyy = real(0.);
    xizz = real(0.);
    num  = lu - ll + 1;
    a.resize(num, atom_fluid);         
    rel.resize(num, VO);

    FOR_EACH_ATOM_i {
        a[i] = atom(type, ll+i, mass, R[ll+i]);
    }

    init();

}

void particle::init()
{
    
    q0.w = 1.0;

    FOR_EACH_ATOM_i   {
        r0 += a[i].r0;  
        xixx += a[i].r0.y * a[i].r0.y + a[i].r0.z * a[i].r0.z;
        xiyy += a[i].r0.z * a[i].r0.z + a[i].r0.x * a[i].r0.x;
        xizz += a[i].r0.x * a[i].r0.x + a[i].r0.y * a[i].r0.y;
    }

    r0 *= real(1.0 / num);
    xixx -= num * (r0.y*r0.y + r0.z*r0.z);
    xiyy -= num * (r0.z*r0.z + r0.x*r0.x);
    xizz -= num * (r0.x*r0.x + r0.y*r0.y);


    FOR_EACH_ATOM_i {
        rel[i] = a[i].r0 - r0;
    }
}


void particle::atmmol()
{
    vec3 fs, rs, ax, ay, az;

    f = VO;
    t = VO;

    FOR_EACH_ATOM_i
    {
        fs = a[i].f;
        rs = a[i].r0 - r0;
        if( PDX ) rs.x -= XSIZE * int(rs.x * XI2 - 1.);
        if( PDY ) rs.y -= YSIZE * int(rs.y * YI2 - 1.);
        if( PDZ ) rs.z -= ZSIZE * int(rs.z * ZI2 - 1.);

        f += fs;
        t.x += rs.y * fs.z - rs.z * fs.y;
        t.y += rs.z * fs.x - rs.x * fs.z;
        t.z += rs.x * fs.y - rs.y * fs.x;
        
    }

    fp = f;
    
    ax.x = q0.w*q0.w + q0.x*q0.x - q0.y*q0.y - q0.z*q0.z;
    ax.y = 2.0 * ( q0.x * q0.y + q0.w * q0.z );
    ax.z = 2.0 * ( q0.x * q0.z - q0.w * q0.y );
    
    ay.x = 2.0 * ( q0.x * q0.y - q0.w * q0.z );
    ay.y = q0.w*q0.w - q0.x*q0.x + q0.y*q0.y - q0.z*q0.z;
    ay.z = 2.0 * ( q0.y * q0.z + q0.w * q0.x );

    az.x = 2.0 * ( q0.x * q0.z + q0.w * q0.y );
    az.y = 2.0 * ( q0.y * q0.z - q0.w * q0.x );
    az.z = q0.w*q0.w - q0.x*q0.x - q0.y*q0.y + q0.z*q0.z;
    
    tp.x = ax * t;
    tp.y = ay * t;
    tp.z = az * t;

}


void particle::molatm()
{
    vec3 ax, ay, az;

    ax.x = q0.w*q0.w + q0.x*q0.x - q0.y*q0.y - q0.z*q0.z;
    ax.y = 2.0 * ( q0.x * q0.y + q0.w * q0.z );
    ax.z = 2.0 * ( q0.x * q0.z - q0.w * q0.y );
    
    ay.x = 2.0 * ( q0.x * q0.y - q0.w * q0.z );
    ay.y = q0.w*q0.w - q0.x*q0.x + q0.y*q0.y - q0.z*q0.z;
    ay.z = 2.0 * ( q0.y * q0.z + q0.w * q0.x );

    az.x = 2.0 * ( q0.x * q0.z + q0.w * q0.y );
    az.y = 2.0 * ( q0.y * q0.z - q0.w * q0.x );
    az.z = q0.w*q0.w - q0.x*q0.x - q0.y*q0.y + q0.z*q0.z;

    FOR_EACH_ATOM_i {
        
        a[i].r0.x = r0.x + ax.x * rel[i].x + ay.x * rel[i].y + az.x * rel[i].z;
        a[i].r0.y = r0.y + ax.y * rel[i].x + ay.y * rel[i].y + az.y * rel[i].z;
        a[i].r0.z = r0.z + ax.z * rel[i].x + ay.z * rel[i].y + az.z * rel[i].z;
     
    }


    if( PDX )
	   FOR_EACH_ATOM_i 
                a[i].periodicx();
    if( PDY )
	   FOR_EACH_ATOM_i  
                a[i].periodicy();
    if( PDZ )
	  FOR_EACH_ATOM_i 
                a[i].periodicz();

}

void particle::predict(){

    static real c1 = Dt;
    static real c2 = Dt * Dt / 2.0;
    static real c3 = Dt * Dt * Dt / 6.0;
    static real c4 = Dt * Dt * Dt * Dt / 24.0;

    r0 += c1 * r1 + c2 * r2 + c3 * r3;
    r1 += c1 * r2 + c2 * r3;
    r2 += c1 * r3;
    periodicbc();

    q0 += c1 * q1 + c2 * q2 + c3 * q3 + c4 * q4;;
    q1 += c1 * q2 + c2 * q3 + c3 * q4;
    q2 += c1 * q3 + c2 * q4;
    q3 += c1 * q4;

    o0 += c1 * o1 + c2 * o2 + c3 * o3 + c4 * o4;;
    o1 += c1 * o2 + c2 * o3 + c3 * o4;
    o2 += c1 * o3 + c2 * o4;
    o3 += c1 * o4;

}

void particle::correct(){
    
    static real c1 = Dt;
    static real c2 = Dt * Dt / 2.0;
    static real c3 = Dt * Dt * Dt / 6.0;
    static real c4 = Dt * Dt * Dt * Dt / 24.0;

    
    static real ctran0 = 1.00 / 6.00 * c2;
    static real ctran1 = 5.00 / 6.00 * c2 / c1;
    static real ctran3 = 1.00 / 3.00 * c2 / c3;
    static real crot0  = 251. / 720. * c1;
    static real crot2  = 11.0 / 12.0 * c1 / c2;
    static real crot3  = 1.00 / 3.00 * c1 / c3;
    static real crot4  = 1.00 / 24.0 * c1 / c4;
    
    vec3  r2i, corr, o1i, ocorr;
    vec4  q1i, qcorr;
       
    r2i  = real(1.0 / mass) * fp;
    corr = r2i - r2;

    r0   = ctran0 * corr;
    r1  += ctran1 * corr;
    r2   = r2i;
    r3  += ctran3 * corr;

    periodicbc();

    q1i.w = ( -q0.x * o0.x - q0.y * o0.y - q0.z * o0.z ) * 0.5;
    q1i.x = (  q0.w * o0.x - q0.z * o0.y - q0.y * o0.z ) * 0.5;
    q1i.y = (  q0.z * o0.x - q0.w * o0.y - q0.x * o0.z ) * 0.5;
    q1i.z = ( -q0.y * o0.x - q0.x * o0.y - q0.w * o0.z ) * 0.5;
    qcorr = q1i - q1;
    q0   += crot0 * qcorr;
    q1    = q1i;
    q2   += crot2 * qcorr; 
    q3   += crot3 * qcorr;
    q4   += crot4 * qcorr;

    o1i.x = ( tp.x + o0.y * o0.z * (xiyy - xizz) ) / xixx;
    o1i.y = ( tp.y + o0.z * o0.x * (xizz - xixx) ) / xiyy;
    o1i.z = ( tp.z + o0.x * o0.y * (xixx - xiyy) ) / xizz;
    corr  = o1i - o1;
    o0   += crot0 * ocorr;
    o1    = o1i;
    o2   += crot2 * ocorr; 
    o3   += crot3 * ocorr;
    o4   += crot4 * ocorr;


}

void particle::binning(){

    FOR_EACH_ATOM_i 
        a[i].binning();

}

void particle::tether(){

    FOR_EACH_ATOM_i
        a[i].tether();

}

void particle::periodicbc(){
	
    if( PDX ) r0.x -= XSIZE * int(r0.x * XI2 - 1.);
    if( PDY ) r0.y -= YSIZE * int(r0.y * YI2 - 1.);
    if( PDZ ) r0.z -= ZSIZE * int(r0.z * ZI2 - 1.);
   	
}

void particle::addforce(vec3 force){
	
    f += force;
}

void particle::clearforce(){
	
    f = VO;
}


void particle::info()
{
    cout<<"-------------------particle Info-------------------"<<endl;
    cout<<"|\tparticle Type |  "<<type<<endl;
    cout<<"|\tAtom Number   |  "<<num<<endl;
    cout<<"|\tID Range      |  "<<ll<<"-"<<lu<<endl;
    cout<<"|\tR0            |  ("<<r0.x<<", "<<r0.y<<", "<<r0.z<<")"<<endl;
    cout<<"|\tR1            |  ("<<r1.x<<", "<<r1.y<<", "<<r1.z<<")"<<endl;
    cout<<"---------------------------------------------------"<<endl;

}
